#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>

using namespace std;
using namespace cv;

void InputImage(Mat &left, Mat &right)
{
    left=imread("../left.png",IMREAD_ANYCOLOR);
    right=imread("../right.png",IMREAD_ANYCOLOR);
    
    if(left.data==NULL||right.data==NULL){
        cout<<"can't input the image"<<endl;
        return ;    
    }
    
    imshow("left raw image",left);
    imshow("rigth raw image",right);
}

void UndisImage(const Mat &left,const Mat &right, Mat &unleft, Mat &unright)
{
    Mat Kl=(Mat_<double>(3,3)<< 8.6447560726240727e+02, 0, 3.1396077284379163e+02,
	0, 7.9990966684818886e+02, 4.1785668776720030e+02,
	0, 0, 1);
    Mat Kr=(Mat_<double>(3,3)<<8.3828754655778573e+02, 0, 3.2434901263473841e+02,
	0, 7.8106334038218915e+02, 4.4025865037526029e+02,
	0, 0, 1);
    Mat Dl=(Mat_<double>(1,5)<<-8.3290443472456477e-01, -1.6141488144887639e+00, -2.0021554243964865e-01, 
	-5.0519644795720990e-03, 3.9975929024497163e+00);
    Mat Dr=(Mat_<double>(1,5)<<-6.9576263466271460e-01, -9.7033517758028354e-01, -1.6926908600014581e-01,
	-4.1180802214907403e-03, 1.9769688036876045e+00);
    undistort(left,unleft,Kl,Dl,Kl);
    undistort(right,unright,Kr,Dr,Kr);
    imwrite("../left_undistort_image.png",unleft);
    imwrite("../right_undistort_image.png",unright);
}  


void stereo_match(const Mat &unleft,const  Mat &unright)
{
    /*************************
     * 
    rect roi1=rect(0,0,unleft.rows,unleft.cols);
    rect roi2=rect(0,0,unright.rows,unright.cols);
    Ptr<StereoBM> bm = StereoBM::create(16,9);//局部的BM;
        // bm算法
    bm->setROI1(roi1);//左右视图的有效像素区域 在有效视图之外的视差值被消零
    bm->setROI2(roi2);
    bm->setPreFilterType(CV_STEREO_BM_XSOBEL);
    bm->setPreFilterSize(9);//滤波器尺寸 [5,255]奇数
    bm->setPreFilterCap(31);//预处理滤波器的截断值 [1-31] 
    bm->setBlockSize(SADWindowSize > 0 ? SADWindowSize : 15);//sad窗口大小
    bm->setMinDisparity(0);//最小视差值，代表了匹配搜索从哪里开始
    bm->setNumDisparities(numberOfDisparities);//表示最大搜索视差数
    bm->setTextureThreshold(10);//低纹理区域的判断阈值 x方向导数绝对值之和小于阈值
    bm->setUniquenessRatio(15);//视差唯一性百分比  匹配功能函数
    bm->setSpeckleWindowSize(100);//检查视差连通域 变化度的窗口大小
    bm->setSpeckleRange(32);//视差变化阈值  当窗口内视差变化大于阈值时，该窗口内的视差清零
    bm->setDisp12MaxDiff(-1);// 1
    
	****/
    int window_size = 5;
    int min_disp = 0;
    int num_disp = 112 - min_disp;
    int max_disp = min_disp + num_disp;
    
    Ptr<StereoSGBM> sgbm = StereoSGBM::create(0,16,3);//全局的SGBM;
     // sgbm算法
        sgbm->setPreFilterCap(63);//预处理滤波器的截断值 [1-63] 
        int SADWindowSize  = 11;
        int sgbmWinSize =SADWindowSize > 0 ? SADWindowSize : 3;
        sgbm->setBlockSize(sgbmWinSize);
        int cn = 1;
        sgbm->setP1(8*cn*sgbmWinSize*sgbmWinSize);// 控制视差变化平滑性的参数。P1、P2的值越大，视差越平滑。
    //P1是相邻像素点视差增/减 1 时的惩罚系数；P2是相邻像素点视差变化值大于1时的惩罚系数。P2必须大于P1
        sgbm->setP2(32*cn*sgbmWinSize*sgbmWinSize);
        sgbm->setMinDisparity(0);//最小视差值，代表了匹配搜索从哪里开始
        sgbm->setNumDisparities(num_disp);//表示最大搜索视差数
        sgbm->setUniquenessRatio(10);//表示匹配功能函数
        sgbm->setSpeckleWindowSize(100);//检查视差连通域 变化度的窗口大小
        sgbm->setSpeckleRange(32);//视差变化阈值  当窗口内视差变化大于阈值时，该窗口内的视差清零
        sgbm->setDisp12MaxDiff(1);// 1
        Mat disp;
        sgbm->compute(unleft, unright, disp);

        imwrite("../disp.png",disp);
}

int main(int argc, char **argv) {
    
    
    Mat leftraw,rightraw,leftimage,rightimage;
    InputImage(leftraw,rightraw);  
    UndisImage(leftraw,rightraw,leftimage,rightimage);
    stereo_match(leftimage,rightimage);
    
    return 0;
}
